#include "Quaternion.hpp"

#ifndef __QUATERNION_CPP__
#define __QUATERNION_CPP__

namespace asteroids
{

template <class T>
Quaternion<T>::Quaternion()
{
        // Default Quaternion
        m_x = 1;
        m_y = 0;
        m_z = 0;
        m_w = 0; 
}

template <class T>
Quaternion<T>::~Quaternion()
{
        // Do nothing
}

template <class T>
Quaternion<T>::Quaternion(Vertex<T> vec, T angle)
{
        // Calculate the quaternion
        fromAxis(vec, angle);
}

template <class T>
Quaternion<T>::Quaternion(T _x, T _y, T _z, T _angle)
{
        // Set the values
        m_x = _x;
        m_y = _y;
        m_z = _z;
        m_w = _angle;
}

template <class T>
void Quaternion<T>::fromAxis(Vertex<T> axis, T angle)
{
        float sinAngle;
        angle *= 0.5f;

        // Create a copy of the given vector and normalize the new vector
//        Vertex<T> vn(axis.m_x, axis.m_y, axis.m_z);
        Normal<T> vn(axis[0], axis[1], axis[2]);
/*        try {
            vn.normalize();
        } catch (std::invalid_argument &ia) {
            std::cout << ia.what() << std::endl;
        }*/
 
        // Calculate the sinus of the given angle
        sinAngle = sin(angle);
 
        // Get the quaternion
        m_x = (vn[0] * sinAngle);
        m_y = (vn[1] * sinAngle);
        m_z = (vn[2] * sinAngle);
        m_w = cos(angle);
}

template <class T>
Quaternion<T> Quaternion<T>::getConjugate()
{
        // Conjugate the given quaternion
        return Quaternion<T>(-m_x, -m_y, -m_z, m_w);
}

template <class T>
Quaternion<T> Quaternion<T>::operator* (const Quaternion<T> rq)
{
        // Calculate the new quaternion
        return Quaternion<T>(m_w * rq.m_x + m_x * rq.m_w + m_y * rq.m_z - m_z * rq.m_y,
                                m_w * rq.m_y + m_y * rq.m_w + m_z * rq.m_x - m_x * rq.m_z,
                                m_w * rq.m_z + m_z * rq.m_w + m_x * rq.m_y - m_y * rq.m_x,
                                m_w * rq.m_w - m_x * rq.m_x - m_y * rq.m_y - m_z * rq.m_z);
}

template <class T>
Vertex<T> Quaternion<T>::operator* (Vertex<T> vec)
{
        // Copy the vector and normalize the new vector
//        Vertex<T> vn(vec);
        Normal<T> vn(vec);
/*        try {
            vn.normalize();
        } catch (std::invalid_argument &ia) {
            std::cout << ia.what() << std::endl;
        }*/

        // Fill the first quaternion and...
        Quaternion<T> vecQuat, resQuat;
        vecQuat.m_x = vn[0];
        vecQuat.m_y = vn[1];
        vecQuat.m_z = vn[2];
        vecQuat.m_w = 0;

        // calculate the new quaternion
        resQuat = vecQuat * getConjugate();
        resQuat = *this * resQuat;
        return (Vertex<T>(resQuat.m_x, resQuat.m_y, resQuat.m_z));
}

}

#endif